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1.  Introduction 

This  paper  presents  an  implementation  of  a  new  motion  planning  algorithm  for  general 
robot  manipulators  moving  among  three-dimensional  polyhedral  obstacles.  The  algo¬ 
rithm  has  a  number  of  advantages:  it  is  simple  to  implement,  it  is  fast  for  manipulators 
with  few  degrees  of  freedom,  it  can  deal  with  manipulators  having  many  degrees  of  free¬ 
dom  (including  redundant  manipulators),  and  it  can  deal  with  cluttered  environments 
and  non-convex  polyhedral  obstacles.  An  examples  of  a  path  obtained  from  an  imple¬ 
mentation  of  the  algorithm  is  shown  in  Figure  1. 

The  ability  to  automatically  plan  collision-free  motions  for  a  manipulator  given 
geometric  models  of  the  manipulator  and  the  task  is  one  of  the  capabilities  required  to 
achieve  task-level  robot  programming  [15].  Task-level  programming  is  one  of  the  principal 
goals  of  research  in  robotics.  It  is  the  ability  to  specify  the  robot  motions  required  to 
achieve  a  task  in  terms  of  task-level  commands,  such  as  “Insert  pin-A  in  hole-B”,  rather 
than  robot-level  commands,  such  as  “Move  to  0.1,0.35,1.6”. 

The  motion-planning  problem ,  in  its  simplest  form,  is  to  find  a  path  from  a  specified 
starting  robot  configuration  to  a  specified  goal  configuration  that  avoids  collisions  with  a 
known  set  of  stationary  obstacles.  Note  that  this  problem  is  significantly  different  from, 
ano  |uite  t  bit  harder  th;  i,  the  collision  detection  problem:  detecting  whether  a  known 
rob(  ■.  con*  guration  or  a  oath  would  cause  a  coll  -ion  l.  4  .  Motion  planning  is  also 
diffi  -ent  from  on-line  obstacle  avoidance  modifying  a  known  robot  path  so  as  to  avoid 
unfi  '•eseen  obstacle-  6,  9  10,  11]. 

Although  general-pu- pose  task-level  programming  is  still  many  years  away,  some 
of  toe  techniques  developed  for  task-level  programming  are  relevant  to  existing  robot 
applications.  There  is,  for  example,  increasing  emphasis  among  major  robot  users  on 
developing  techniques  for  off-line  programming,  by  human  programmers,  using  CAD 
models  of  the  manipulator  and  the  task.  In  many  of  these  applications  motion  plan¬ 
ning  plays  a  central  role.  Arc  welding  is  a  good  example:  specifying  robot  paths  for 
welding  along  complex  three-dimensional  paths  is  a  timr-  -nsuming  and  tedious  process. 
The  development  of  practical  motion-planning  algorithms  could  reduce  significantly  the 
programming  time  for  these  applications. 

A  great  deal  of  research  has  been  devoted  to  the  motion-planning  problem  within 
the  last  five  to  eight  years,  e  g.,  [2,  3,  5,  7,  8,  12,  13,  14,  16,  17.  19,  20] .  But,  few  of  these 
methods  are  simple  enough  and  powerful  enough  to  be  practical.  Practical  algorithms 
are  particularly  scarce  for  manipulators  made  up  of  revolute  joints,  the  most  popular 
type  of  industrial  robot.  I  know  of  only  two  previous  motion-planning  algorithms  that 
are  both  efficient  and  reasonably  general  for  revolute  manipulators  with  three  or  more 
degrees  of  freedom  [2,  7],  Brooks’s  algorithm  |2  has  demonstrated  impressive  results, 
but  is  fairly  complex.  Faverjon’s  algorithm  7],  on  the  other  hand,  is  appealingly  simple. 
The  basic  approach  of  the  algorithm  described  here  is  closely  related  to  the  method 
described  by  Faverjon.  Many  of  the  details  of  the  present  algorithm,  however,  especially 
the  treatment  of  three-dimensional  constraints  and  the  free  space  representation,  are  new 
and  more  general 


figure  1  (a)  Sample  configurations  along  a  path  for  all  six  links  of  a  Unimation  Puma  manipulator 
obtained  usmg  the  algorithm  described  here.  The  total  planning  time  on  a  Symbolics  3600  L,sp 
Machine  without  floating-point  hardware  was  40  seconds.  The  incremental  cost  for  planning 
another  path  in  the  same  environment  is  approximately  2  seconds 


Figure  2  (a)  A  closeup  of  the  initial  configuration  of  the  manipulator  for  the  path  in  Figure  lb 
(from  a  different  viewpoint)  (b)  A  closeup  of  the  final  configuration  of  the  manipulator  for  the 
path  in  Figure  lb  Note  that  the  full  polchedral  description?  of  the  arm  and  end-effector  are 
used  in  the  algorithm 


The  approach  taken  in  this  algorithm  is  similar  to  that  of  [7,  8,  12,  13!  in  that  it 
involves  quantizing  joint  angles.  It  differs  in  this  respect  from  exact  algorithms  such  as 
!  17,  19j.  On  the  other  hand,  the  quantization  approach  lends  itself  readily  to  efficient 
computer  implementation. 

The  purpose  of  this  paper  is  to  show  that  motion  planning  for  general  manipulators 
can  be  both  simple  and  relatively  efficient  in  most  practical  cases.  There  is  no  reason 
why  motion  planning  should  be  any  less  practical  than  computing  renderings  of  three 
dimensional  solids  in  computer  graphics.  In  both  cases,  there  are  many  simple  numerical 
computations  that  can  benefit  from  hardware  support.  In  fact,  it  is  worth  noting  that 
in  the  examples  in  Figure  1  it  took  longer  to  compute  the  hidden-surface  displays  in  the 
figures  than  to  compute  the  paths. 

2.  The  Basic  Approach:  Slice  Projection 

The  configuration  of  a  moving  object  is  any  set  of  parameters  that  completely  specify  the 
position  of  every  point  on  the  object.  Configuration  space  (C-space)  is  the  space  of  con¬ 
figurations  of  a  moving  object  The  set  of  joint  angles  of  a  robot  manipulator  constitute 
a  configuration.  Therefore,  a  robot’s  joint  space  is  a  configuration  pace.  The  cart  sian 
parameters  of  the  robot’s  end  effec’or,  on  thi  other  hand,  do  not  usually  constit  te  a 
configuration  because  of  the  multiplicity  of  so  utions  to  a  rol  ot’s  inverse  kit  emati  -.  It 
is  possible  to  map  the  obstacles  in  the  robot’s  workspace  int,  its  configuration  spa  »■  3. 
4,  5.  13,  14  .  These  C-.-pace  obstacle  *  represent  those  configurations  of  the  m<  ving  c  ject 
that  would  cause  collisions.  Fret  space  is  defined  to  be  the  lomplement  of  <he  C-  pace 
obstacles 

Motion  planning  requires  an  explicit  characterization  of  the  robot’s  free  space.  The 
characterization  may  not  be  complete,  for  example,  it  may  cover  only  a  subset  of  the 
free  space.  But,  without  a  characterization  of  the  free  space,  one  is  reduced  to  trial 
and  error  methods  to  find  a  path.  In  this  paper  we  show  how  to  compute  approximate 
characterizations  of  the  free  space  for  simple  manipulators.  Bj  simple  manipulators  we 
mean  manipulators  composed  of  a  non-branching  sequence  of  links  connected  by  either 
revolute  or  prismatic  joints  (see  18  for  a  treatment  of  the  kinematics  of  simple  manip¬ 
ulators).  We  restrict  the  position  of  link  zero  of  a  simple  manipulator  to  be  fixed  Most 
industrial  manipulators  (not  including  parallel-jaw  grippers)  are  simple  manipulators  in 
this  sense. 

The  C-space  obstacles  for  a  manipulator  with  n  joints  are,  in  general,  n-dimensional 
volumes.  Let  C  denote  an  n  dimensional  C-space  obstacle  for  a  manipulator  with  n  joints. 
We  represent  approximations  of  C  by  the  union  of  n-  1  dimensional  slice  projections  13. 
14  .  Each  n  -  1  dimensional  configuration  in  a  slice  projection  of  C  represents  a  range 
of  n  dimensional  configurations  (differing  only  in  the  value  of  a  single  joint  parameter) 
that  intersects  C 

A  slice  projection  of  an  n  dimensional  C-space  obstacle  is  defined  by  a  range  of  values 
for  one  of  the  defining  parameters  of  the  C-space  and  an  n  1  dimensional  volume  Let 
the  q  =  (9i,---,9n)  denote  a  configuration,  where  each  q,  is  a  joint  parameter.  ea<  h  of 
which  measures  either  angular  displacement  (for  revolute  joints)  or  linear  displacement 


Figure  3.  Slice  Projection  of  a  three-dimensional  obstacle  into  a  list  of  two-dimensional  slices 
that  are  in  turn  represented  by  one-dimensional  slices. 


(for  prismatic  joints).  Let  {q  q,  €  a.  3  }  be  the  set  of  all  configurations  for  which 
q}  6  tcc.  3  and  let  7r,  be  a  projection  operator  such  that 

=  (?l . 7;  -  1  !  -  1 '  -,7n) 

Then,  the  slice  projection  of  the  obstacle  C  for  values  of  q,  G  a,  3  is 

{<1  7;  €  a,  3  }) 

The  definition  of  slice  projection  is  illustrated  in  Figure  ",  In  the  example  above,  joint  j 
above  is  called  the  slice  joint  while  the  other  joints  are  known  as  free  joints. 

Note  that  a  slice  projection  is  a  conservative  approximation  of  a  segment  of  an  n 
dimensional  C-space  obstacle.  An  approximation  of  the  full  obstacle  is  built  as  the  union 
of  a  number  of  n  -  1  dimensional  slice  projections,  each  for  a  different  range  of  values  of 
the  same  joint  parameter  (Figure  3)  Each  of  the  n  -  1  dimensional  slice  projections,  in 
turn,  can  be  approximated  by  the  union  of  n  -  2  dimensional  slice  projections  and  so  on, 
until  we  have  a  union  of  one  dimensional  volumes,  that  is,  linear  ranges.  This  process  is 
illustrated  graphically  in  Figure  3.  Note  that  the  slice  projection  can  be  continued  one 
more  step  until  only  zero  dimensional  volumes  (points)  remain,  but  this  is  wasteful. 

Consider  a  simple  two-link  planar  manipulator  whose  joint  parameters  are  and 
q-j.  C-space  obstacles  for  such  a  manipulator  are  two  dimensional  The  one  dimensional 
slice  projection  of  a  C-space  obstacle  C  for  </|  ■  j n,3  is  some  set  of  linear  ranges  {  R, } 
for  q2  The  ranges  must  be  such  that,  if  there  exists  a  value  of  <72,  call  it  jJ .  and  a  value 
qi  £  [a,/?j,  call  it  f,  for  which  (s",  w)  C.  then  jJ  is  in  one  of  the  R ,  (Figure  3). 
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Figure  4.  (a)  Two  link  revolute  manipulator  and  ('Ostades,  (b)  Tv  >  dimensional  C  space  ith 

obstacles  approximated  by  a  list  of  one  dimensional  slice  projections  (shown  dark).  The  u  tial 
and  final  positions  of  the  manipulator  are  shown  in  the  input  space  and  the  C-space. 


A  representation  of  a  configuration  space  with  obstacles  is  illustrated  in  Figure  4b 
for  the  two  link  manipulator  and  obstacles  shown  in  Figure  4a.  The  actual  configuration 
space  is  the  surface  of  a  torus  since  the  top  and  bottom  edge  of  the  diagram  coincide 
(0  =  2n),  as  do  the  left  and  right  edge.  The  obstacles  are  approximated  as  a  set  of  q2 
ranges  (shown  dark)  for  a  set  of  values  of  q j .  The  resolution  is  2  degrees  along  the  g, 
axis. 

If  the  manipulator  has  three  links,  its  configuration  space  can  be  constructed  as 
follows: 


1.  Ignore  links  beyond  link  1.  Find  the  ranges  of  legal  values  of  q\  by  considering 
rotations  of  link  1  around  the  fixed  base 

2.  Sample  the  legal  range  of  q j  at  the  specified  resolution.  Do  steps  3  through  5  for 
each  of  the  value  ranges  of  q\. 

3.  Ignore  links  beyond  link  2  Find  the  ranges  of  legal  values  of  qj  by  considering 
rotating  link  2  around  the  positions  of  joint  2  determined  by  the  current  value  range 
of  9,. 


7 


4.  Sample  the  legal  range  of  q3  at  the  specified  resolution.  Do  step  5  for  each  of  these 
value  ranges  of  qj. 

5  Find  the  ranges  of  legal  values  of  q$  by  considering  rotating  link  3  around  the  position 
of  joint  3  determined  by  the  current  value  ranges  of  q j  and 


Some  sample  slices  from  a  configuration  space  computed  in  this  way  can  be  seen  in  Figure 
5. 

Note  that  the  process  described  above  is  an  instance  of  a  simple  recursive  process 


To  compute  C-space(i): 

1.  Ignore  links  beyond  link  i.  Find  the  ranges  of  legal  values  of  by  considering 
rotating  link  «  around  the  positions  of  joint  i  determined  by  the  current  value  ranges 
of  .  ,qt-i 

2.  If  «  =  n  then  stop,  else  sample  the  legal  range  of  q,  at  the  specified  resolution. 
Compute  C-space(»  —  1)  for  each  of  these  value  ranges  of  qt. 


Observe  that  the  basic  computation  to  be  done  is  that  of  determining  the  ranges  of 
legal  values  for  a  joint  parameter  given  ranges  of  values  of  the  previous  joints.  This 
computation  is  the  subject  of  Section  3. 

The  recursive  nature  of  the  C-space  computation  calls  for  a  recursive  data  structure 
to  represent  the  C-space.  The  current  implementation  uses  a  tree  whose  depth  is  n  -  1, 
where  n  is  the  number  of  joints,  and  whose  branching  f-  "tor  is  the  number  of  intervals 
into  which  the  legal  joint  parameter  range  for  each  joint  is  divided  (Figure  6).  The  leaves 
of  the  tree  are  ranges  of  legal  (or  forbidden)  values  for  the  joint  parameter  n.  Many  of 
the  internal  nodes  in  the  tree  will  have  no  descendants  because  they  produce  a  collision 
of  some  link  «  <  n. 

The  main  advantage  of  a  representation  method  built  on  recursive  slice  projection 
is  its  simplicity.  All  operations  on  the  representation  boil  down  to  dealing  with  linear 
ranges,  for  which  very  simple  and  efficient  implementations  are  possible  The  disadvan¬ 
tages  are  the  loss  of  accuracy,  and  the  rapid  increase  of  storage  and  processing  time 
with  dimensionality  of  the  C-space.  Contrast  this  approach  with  one  that  represents  the 
boundaries  of  the  obstacles  by  their  defining  equations  [4,  5  .  Using  the  defining  equations 
is  cleaner  and  more  accurate,  but  the  algorithms  for  dealing  with  interactions  between 
obstacle  boundaries  are  very  complex  I  believe  that  the  simplicity  of  slice  projection 
outweighs  its  drawbacks.  These  drawbacks  can  be  significantly  reduced  by  exercising 
care  in  the  implementation  of  the  algorithms. 
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figj-*.  t  '  hr  rr’un.xr  "attire  I't  'hr  <  -t  *•  J *ads  to  a  rec  ursive  dat a  structure:  an  n-level 
•r-t-  *  '  «••  i-jvc.  r*‘pr*--t>ni  .--pa!  rang*"  •  !  <  ■  nfi?  oat  ions  for  th*-  robot  manipulator. 


3.  Slice  Projections  for  Polygons 

7  be  kh  -wj  in  our  approach  is  computing  one  dimensional  slice  projections  of  C-space 
"••Mi'  ie»  That  i'  determining  the  range  of  forbidden  ...lues  of  one  joint  parameter, 
given  rang*"  ,f  value-  for  all  previous  joint  parameters  We  will  illustrate  how  these 
range-  tr  av  be  totnputed  bv  r..tisidering  the  case  of  planar  revolute  manipulators  and 
op-t a  I*  -  \\»  w  ill  fir--  di*>ru<s  this  problem  informally  and  then  derive  the  solution  from 
t  tie  *- j  ,at|'  ti-  of  (  --u'f  icev 

3.i.  A  geometric  view 

As- 1, me  t  r, at  joint  k  a  rev  oiut  e  joint  i-  t  he  free  joint  for  a  one-uimensional  slice  projection 
and  that  tin  predion*-  joints  are  fixed  at  known  values  Note  that  we  assume,  for  now, 
that  the  previous  joint-  are  fixed  a’  ,-t ngh  values  rather  than  ranges  of  values;  we  will 
-*•(•  iti  *s»‘*tion  tiovx  to  relax  t hi-  re-iri<tion  We  require  that  the  configuration  of  the 
hr-t  k  I  hnk-  be  -afe  that  is  no  link  intersects  an  obstacle  This  is  guaranteed  by  the 
vv'1  re<  ur-ive  c  i  .mput  a’ mil  we  i n  >ect|on  ’J  (Tveri  these  assumptions,  we  need  to  find 


Figure  7.  Contact  conditions  for  computing  one  dimensional  slice  projections:  (a)  Vertex  of 
obstacle  and  edge  of  link  (b)  vertex  of  link  and  edge  of  obstacle.  The  circles  indicate  the  path 
of  the  vertices  as  the  link  rotates  around  the  specified  joint. 

the  ranges  of  values  of  the  single  joint  parameter  that  are  forbidden  by  the  presence 
of  objects  in  the  workspace. 

The  ranges  of  forbidden  values  for  q^  will  be  bounded  by  angles  where  link  k  is 
just  touching  an  obstacle.  For  polygonal  links  moving  among  polygonal  obstacles,  the 
extremal  contacts  happen  when  a  vertex  of  one  object  is  in  contact  with  an  edge  of 
another  object.  Therefore,  the  first  step  in  computing  the  forbidden  ranges  for  q *  is  to 
identify  those  critical  values  of  for  which  some  obstacle  vertex  is  in  contact  with  a  link 
edge  or  some  link  vertex  is  in  contact  with  an  obstacle  edge  (Figure  7). 

The  link  is  constrained  to  rotate  about  its  joint,  therefore  every  point  on  the  link 
follows  a  circular  path  when  the  link  rotates.  The  link  vertices,  in  particular,  are  con¬ 
strained  to  known  circular  paths.  The  intersection  of  these  paths  with  obstacle  edges 
determine  some  of  the  critical  values  of  g*,  for  example,  B  in  Figure  7.  As  the  link 
rotates,  the  obstacle  vertices  also  follow  known  circular  paths  relative  to  the  link.  The 
intersection  of  these  circles  with  link  edges  determine  the  remaining  critical  values  for 
qk,  for  example,  A  in  Figure  7. 

Determining  whether  a  vertex  and  an  edge  segment  can  intersect  requires  first  in¬ 
tersecting  the  circle  traced  out  by  the  vertex  and  the  infinite  line  supporting  the  edge 
to  compute  the  potential  intersection  points.  The  existence  of  such  an  intersection  is 
v  a  necessary  condition  for  a  contact  between  link  and  obstacle,  but  it  is  not  sufficient. 
Three  additional  constraints  must  hold  (Figure  8):  in-edge  constraint  -  the  intersection 
point  must  be  within  the  finite  edge  segment,  not  just  the  the  line  supporting  the  edge; 
orientation  constraint  -  the  orientation  of  the  edges  at  the  potential  contact  must  be 
compatible,  that  is,  the  edges  that  define  the  contact  vertex  must  both  be  outside  of  the 
contact  edge;  reachability  constraint  -  for  non-convex  objects,  there  must  not  be  other 
contacts  that  prevent  reaching  this  point. 

The  in-edge  constraint  can  be  tested  trivially  given  the  potential  contact  point  and 
the  endpoints  of  the  contact  edge.  Since  we  know  that  the  contact  point  is  on  the  line 
of  the  edge,  all  that  remains  to  be  determined  is  whether  it  lies  between  the  endpoints 


Figure  8.  Given  the  intersection  of  a  vertex  circle  and  an  edge  line,  the  following  conditions 
must  be  met  for  a  feasible  contact:  (a)  The  contact  must  be  in  the  edge  segment,  contact  1 
satisfies  this  but  l'  does  not  (b)  The  edges  that  define  the  contact  vertex  must  both  be  outside 
of  the  contact  edge,  contact  1  satisfies  this  but  contact  2  does  not.  (c)  The  contact  must  be 
reachable,  contact  1  satisfies  this,  but  contact  3  does  not  (this  condition  is  only  relevant  for 
non-convex  objects). 

of  the  edge.  This  can  be  done  by  ensuring  that  the  z  and  y  coordinates  of  the  contact 
point  are  within  the  range  of  x  and  y  coordinates  defined  by  the  edge  endpoints.  Note 
that  for  contacts  involving  link  edges  and  obstacle  vertices,  the  position  of  the  endpoints 
of  the  link  edge  must  be  rotated  around  the  joint  position  by  the  computed  value  of  the 
joint  angle  at  the  contact. 

The  orientation  constraint  can  also  be  tested  simply  All  that  is  required  is  that  the 
two  edges  forming  the  contact  vertex  be  on  the  outside  of  the  contact  edge.  Polygon 
edges  are  typically  oriented  so  that  they  revolve  in  a  counterclockwise  direction  about 
the  boundary.  Therefore,  the  outside  of  the  polygon  is  on  the  right  of  the  edge  as  we 
traverse  the  boundary.  Given  this,  the  feasibility  of  a  contact  can  be  verified  simply  by 
comparing  the  absolute  orientations  of  the  edges  involved  in  the  contact. 

The  reachability  constraint,  on  the  other  hand,  l  .quires  examining  all  the  contacts 
of  the  link  with  a  given  obstacle  that  satisfy  the  first  two  constraints.  For  each  contact 
angle  q  we  determine  whether  values  of  qk  greater  than  q  cause  collision  or  whether  values 
less  than  q  cause  collision  (Section  3  2).  The  contact  angles  together  with  the  collision 
directions  can  be  merged  to  form  the  ranges  of  forbidden  values  for  9*.  This  process  is 
illustrated  in  Figure  9 

3.2.  Derivation  using  C-surfaces 

The  two  types  of  contacts  (vertex-edge  and  edge- vertex)  give  rise  to  the  two  basic  type 
of  C-space  boundary  (hyper-)surfaces  3,  4,  5,  14  One  type  of  C-surface  (type  A) 
characterizes  the  configuration  of  the  moving  object  for  which  a  vertex  of  the  stationary 
obstacle  is  in  contact  with  the  infinite  line  supporting  an  edge  of  the  moving  object  The 
other  (typf  B)  characterizes  the  configuration  of  the  moving  object  for  which  the  infinite 


Figure  9.  Constructing  ranges  of  forbidden  values  using  the  potential  contact  angles  and  the 
collision  directions. 

line  supporting  an  edge  of  the  stationary  obstacle  is  in  contact  with  a  vertex  of  the 
moving  object.  The  equations  of  such  surfaces  are  parameterized  by  the  configuration 
parameters  of  the  moving  object.  For  planar  polygons,  x,  y,  0  can  be  used  as  configuration 
parameters;  for  manipulators,  the  q<  are  the  configuration  parameters. 

For  a  revolute  joint,  choose  the  coordinate  system  to  be  located  at  the  joint.  The 
coordinate  representation  of  all  of  the  vectors  will  be  relative  to  this  coordinate  system. 
We  represent  a  line  supporting  an  edge  by  an  equation  of  the  form:  ax-td  =  0.  Where  n 
is  the  (outward  pointing)  unit  vector  that  is  normal  to  the  line  and  d  is  the  perpendicular 
distance  to  the  edge  from  the  origin.  The  condition  for  a  vertex  v  being  in  contact  with 
such  a  line  is  simply  n  •  v  —  d  =  0. 

For  a  type  B  contact,  we  are  given  a  link  vertex  whose  initial  position  vector  (for 
qk  =  0)  is  v  and  an  obstacle  edge  whose  line  equation  is  n  •  *  +  d  =  0.  If  the  link  angle 
is  qk ,  the  coordinates  of  the  rotated  link  vertex  are: 

v'  =  (vxcosqk  -  vv  sin?*,  vxsin?fc  -r  vv  cos  qk) 

Substituting  into  the  plane  equation  yields  a  simple  trigonometric  equation  in  qk  (all  the 
other  terms  are  constant): 

( nxvx  +  nyvv)cosg*  -+■  (n„t>x  -  nxvv)  sin  qk  +  d  =  0  (1) 

From  the  definition  of  the  scalar  and  vector  product,  we  have  that 

nxvx  —  riyVy  =  ||vj;  cos0,  nvvx  -  nxvv  =  v  sind> 

where  <j>  is  the  angle  between  n  and  v.  From  this,  it  is  clear  that  the  C-surface  equrtion 
is  merely 

!>  cos [qk  <t>)  -  -d 

The  solution  to  this  equation  is: 


(2) 


Figure  10  illustrates  this  situation.  There  is  one  such  C-surface  for  each  combination  of 
link  vertex  and  obstacle  edge.  Of  course,  only  convex  vertices  need  be  considered;  no 
contact  is  possible  at  a  concave  vertex. 

Using  the  same  notation,  except  that  the  edge  is  a  link  edge  and  the  vertex  an 
obstacle  vertex,  the  equation  for  a  type  A  C-surface  is 

[nxvx  +  nvvv )  cos?*  -  ( nyvx  -  nxvy)  sin  qk  4-  d  =  0  (3) 

The  only  difference  is  the  sign  of  the  coefficient  of  sin  ?*,  this  arises  from  the  fact  that  we 
are  thinking  of  the  obstacle  vertex  as  counter-rotating  while  the  link  stands  still.  That 
is,  the  direction  of  rotation  of  the  vertex  is  the  opposite  of  qk\  this  changes  the  sign  of 
the  sine  of  the  angle.  The  solution  to  this  equation  is: 


There  is  one  such  C-surface  for  each  combination  of  (convex)  obstacle  vertex  and  link 
edge. 

Note  that  there  are  generally  two  solutions  to  each  of  the  equations  (arising  from  the 
arccosine)  since  they  correspond  to  intersections  of  a  circle  traced  out  by  a  vertex  and  an 
infinite  line  supporting  an  edge.  These  solutions,  however,  do  not  necessarily  represent 
feasible  contacts  between  the  link  and  an  obstacle.  The  remaining  constraints  illustrated 
in  Figure  8  must  also  be  satisfied.  Of  course,  when  the  mag  'itude  of  the  argument  to 
the  arccosine  is  greater  than  one,  this  indicates  an  infeasible  contact,  that  is,  the  line  is 
beyond  the  reach  of  the  vertex. 

The  in-edge  constraint  can  be  checked,  as  described  before,  by  computing  the  co¬ 
ordinates  of  the  intersection  point  and  the  positions  of  the  edge  endpoints,  given  the 
computed  values  of  qk- 


Figure  11.  Testing  the  orientation  constraint  for  polygonal  contact 


The  solutions  obtained  from  equations  (2)  and  (4)  must  also  satisfy  the  orientation 
constraint.  One  way  of  testing  this  constraint  is  by  ensuring  that  the  polygon  edges  that 
intersect  at  the  contact  vertex  both  point  outward  from  the  contact  edge.  If  ej  and  e2 
are  the  edge  vectors  pointing  away  from  the  vertex  (Figure  11),  thea  the  orientation 
constraint  boils  down  to 


sign(n  •  *i)  >0,  sign(n  •  e2)  >0 
where  sign(x)  =  x/\x\  for  x  ?£  0  and  0  otherwise. 

The  reachability  constraint  is  handled  as  described  in  Section  3.2.  To  do  that,  we 
must  be  able  to  tell  whether  an  increase  in  qk  will  move  the  contact  away  from  the  contact 
or  further  into  contact.  This  can  be  done  by  computing  the  derivative  of  Eqs.  (1)  and 
(2).  The  left  hand  side  of  these  equations  is  a  measure  of  the  perpendicular  distance  of 
a  vertex  from  an  edge.  The  sign  of  the  derivative  of  this  distance  with  respect  to  qk  will 
indicate  whether  a  change  in  qk  will  move  further  into  contact  or  away.  For  example,  the 
sign  of  the  derivative  of  the  distance  for  a  type  B  contact  (Eq.  (1))  is  determined  by  the 
sign  of  -  sin(?h  +  <p)  evaluated  at  the  value  of  qk  that  gives  rise  to  contact. 

3.3.  The  effect  of  ranges  of  joint  angles 

Our  discussion  thus  far  has  been  limited  to  situations  where  all  the  joints  except  the 
last  have  known  fixed  values.  The  definition  of  one-dimensional  slice  projections  allows 
all  the  joints,  save  one  free  joint,  to  be  within  a  range,  not  just  a  single  value.  We  can 
readily  convert,  the  slice  projection  problem  (for  ranges  of  joint  values)  to  the  simpler 
crossection  projection  problem  (for  single  joint  values)  we  have  already  discussed.  The 
idea  is  to  replace  the  shape  of  the  link  under  consideration  by  the  area  it  sweeps  out  when 
the  joints  defining  the  slice  move  within  their  specified  value  ranges  [13,  14].  Any  safe 
placement  of  the  expanded  link  represents  a  range  of  legal  displacements  of  the  original 
link  within  the  specified  joint  ranges. 

In  most  cases,  instead  of  computing  the  exact  swept  volumes,  we  can  use  a  very 
simple  approximation  method  Assume  the  manipulator  is  positioned  at  the  configuration 
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Figure  12.  The  manipulator  link  can  be  grown  by  a  radius  6k:  the  maximum  cartesian 
displacement  of  any  point  on  the  link  in  response  to  joint  displacements  t,  for  t  <  k. 


defined  by  the  midpoint  of  all  the  joint  value  ranges  specified  for  the  slice  projection. 
Compute  the  magnitude,  6k,  of  the  largest  cartesian  displacement  of  any  point  on  link  k 
in  response  to  any  displacement  within  the  specified  range  of  joint  values.  If  we  “grow” 
each  link  by  its  corresponding  radius  6k,  the  grown  link  includes  the  swept  area. 

A  polygonal  approximation  to  the  grown  link  can  be  obtained  by  computing  the 
“set  sum”  or  “Minkowski  sum”  of  the  link  and  a  polygon  enclosing  a  circle  of  radius  6 
[14].  An  example  of  such  an  grown  manipulator  can  be  seen  in  Figure  12. 

We  can  illustrate  this  approach  by  considering  how  to  compute  6k  for  a  planar 
manipulator  composed  of  n  revolute  joints.  The  motion  of  a  joint  affects  the  displacement 
of  all  subsequent  links.  Therefore,  the  maximum  cartesian  displacement  of  each  link 
depends  on  the  maximum  total  distance  from  any  point  on  a  link  to  the  base  joint  and 
on  the  maximum  angular  displacement  of  the  link.  The  maximum  distance  of  points  on 
link  k  is  the  sum  of  the  distances  between  all  previous  joints  plus  the  distance  of  any 
point  on  link  k  from  joint  k.  The  angular  displacement  of  link  k  in  a  planar  revolute 
manipulator  is  also  the  sum  of  the  angular  displacements  of  all  the  previous  joints.  Given 
the  distance  d  and  the  angle  9,  the  magnitude  of  the  displacement  (chord  of  a  circle)  is 
\/2(l  -  cos0). 

Let  the  allowed  angle  range  for  qk  be  ak  ±  ik;  let  rk  be  the  maximum  distance  of 
any  point  on  link  k  from  joint  k\  and  let  lk  be  the  distance  from  joint  k  to  joint  k  +  1 
The  value  of  6k  is 


:v,< 


1  -  cos 


Because  the  last  link's  motion  is  never  quantized  when  computing  the  C-space,  we  have 
that:  tk  =  0.  This  value  of  6k  is  very  conservative;  it  is  the  largest  displacement  anywhere 
in  the  workspace.  In  fact,  it  corresponds  to  the  displacement  in  a  link  when  all  the  pre¬ 
vious  links  are  fully  outstretched,  that  is,  all  the  a,  =  0 ,j  <  k.  Different  configurations 
would  yield  smaller  values  of  Sk¬ 
in  Figure  12  the  relevant  parameter  values  are:  (\  =  2°,f2  =  2°,cs  =  0,/0  =  0, / 1  - 
17.0, /2  =  17.0,  rj  =  18.44,  r2  =  17.26,  r3  —  5.385.  Therefore,  the  values  of  the  6k  are: 

=  0.644, 6j  =  2.39,63  =  2.749.  Note  the  growth  in  the  value  of  6k  as  the  distance  from 
the  base  increases.  Because  of  this,  one  might  want  to  choose  a  finer  quantization  for 
joints  associated  with  long  links  near  the  base,  for  example,  joint  two  in  our  example. 

In  some  applications,  if  the  f*  are  small,  it  may  be  preferable  to  ignore  the  effect  of 
small  tfc  during  planning  and  simply  check  the  resulting  path  for  collisions.  Of  course, 
if  the  joint  ranges  c*  are  large,  these  gross  approximation  may  be  too  conservative  and 
the  exact  swept  volume  should  be  used. 

3.4.  Prismatic  joints 


I 


The  discussion  above  has  concentrated  on  revolute  joints  but  the  approach  is  not  limited 
to  them.  If  any  of  the  joints  are  prismatic,  only  the  computation  of  one  dimensional 
slices  will  be  different  and,  in  fact,  it  will  be  simpler. 

As  before,  the  key  problem  is  computing  the  critical  value  of  the  joint  parameters 
for  which  a  link  is  in  contact  with  an  obstacle.  These  contacts  involve  contact  of  a  vertex 
and  an  edge.  So,  as  in  the  case  for  revolute  joints,  we  need  is  determine  the  locus  of 
motion  of  link  vertices  relative  to  obstacle  edges  and  the  locus  of  motion  of  obstacle 
vertices  relative  to  link  edges.  For  links  actuated  by  revolute  joints,  we  have  seen  that 
the  vertices  trace  out  circles  For  links  actuated  by  prismatic  joints,  the  points  on  the 
link  trace  out  lines.  Potential  points  of  contact  occur  where  the  lines  defined  by  the 
motion  of  the  vertices  intersect  the  edges  This  operation  replaces  the  intersection  of 
circles  and  lines  in  the  preceding  discussion. 

The  points  of  intersection  must  still  satisfy  the  in-edge,  orientation,  and  reachability 
constraints  Note  that  the  in-edge  constraint  must  now  be  modified  to  check,  not  only 
that  the  intersection  point  is  within  the  finite  edge  segment  of  the  polygon,  but  also  that 
the  contact  is  within  the  range  of  motion  of  the  joint. 


4.  Slice  Projections  for  Polyhedra 


The  basic  approach  described  in  Section  3  carries  over  directly  to  three  dimensional 
manipulators  and  obstacles  There  is  however  orie  significant  difference  there  are  three 
types  of  contacts  possible  between  three  dimensional  polyhedra  The  three  contact  types 


are:  (type  A)  vertex  of  obstacle  and  face  of  link,  (type  B)  vertex  of  link  and  face  of 
obstacle,  and  (type  C)  edge  of  link  and  edge  of  obstacle. 

Let  us  consider  type  B  contacts  first.  Each  revolute  joint  is  characterized  by  an  axis 
of  rotation.  As  the  joint  rotates,  link  vertices  trace  circles  in  a  plane  whose  normal  is 
the  joint  axis.  The  intersection  of  this  circle  with  the  plane  supporting  an  obstacle  face 
defines  two  candidate  points  of  contact  (see  the  appendix)  As  in  the  two-dimensional 
case,  possible  contacts  must  satisfy  three  constraints  to  be  feasible:  in-face  constraint 
the  contact  must  be  within  the  obstacle  face,  orientation  constraint  all  of  the  link  edges 
meeting  at  the  vertex  must  be  outside  of  the  obstacle,  and  reachability  constraint  for 
non-convex  polyhedra.  there  must  not  be  any  earlier  contacts  that  prevent  reaching  this 
one 

The  in-face  constraint  can  be  checked  using  any  of  the  existing  algorithms  for  testing 
whether  a  point  is  in  a  polygon.  The  orientation  constraint  can  be  enforced  by  checking 
that  the  dot  products  of  the  face  normal  w  ith  each  of  the  vectors  from  the  contact  vertex 
to  adjacent  vertices  is  positive  (5_.  The  reachability  constraint  is  enforced  exactly  as  in 
the  two-dimensional  case  by  merging  the  forbidden  angle  ranges. 

Type  A  contacts  are  handled  analogously  to  type  B  contacts  except  that  now  the 
vertex  belongs  to  an  obstacle  and  the  face  to  a  link  The  axis  of  rotation  is  still  that  of 
the  manipulator  joint. 

Detecting  type  C  contacts  require  detecting  the  intersection  of  a  line  (supporting 
a  link  edge)  rotating  about  the  joint  axis  and  a  stationary  line  (supporting  an  obstacle 
edge)  The  solution  for  this  case  can  be  found  in  the  appendix.  Of  course,  an  intersection 
point  must  be  inside  both  edge  segments  to  be  feasible  There  is  also  an  orientation 
constraint  which  is  a  bit  more  difficult  to  derive  than  those  for  type  A  and  B  contacts 
but  not  particularly  difficult  to  check  (for  the  derivation,  see  o'). 

The  appendix  shows  the  details  of  these  computations 

5.  Free  Space  Representation 

Having  obtained  a  conservative  approximation  of  the  C-space  obstacles,  the  free  space  is 
simply  the  complement  of  all  the  obstacles  Since  the  obstacles  are  ultimately  represented 
as  sets  of  linear  ranges,  the  complement  i>  trivial  to  compute.  A  two  dimensional  free 
space,  for  example,  will  be  represented  as  a  list  of  one  dimensional  slices.  Each  slice 
represents  the  ranges  of  legal  values  of  q-,  for  some  small  range  of  values  of  q j.  This  is 
in  itself  a  reasonably  convenient  representation  of  the  free  spare  but  not  very  compact. 
If  we  were  to  try  to  find  paths  through  the  individual  slices  a  great  deal  of  time  would 
be  wasted  searching  through  nearly  identical  slices  A  mot*  compact  representation  is 
called  for.  one  that  raptures  some  of  the  coherence  between  adjacent  slices 

The  free  space  representation  used  m  the  current  implementation  is  made  up  of 
regions  A  region  is  made  up  out  of  overlapping  ranges  from  a  set  of  adjacent  slices 
(Figure  13)  The  area  of  common  overlap  of  all  the  slices  m  a  region  is  rectangular  and 
tailed  the  region's  ktrn'l  In  practice  we  require  some  minimum  overlap  between  slices 
in  the  same  regions  to  avoid  very  narrow  kernel'' 
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Figure  13.  (a)  Region  definition  for  two  link  C-space.  The  rectangular  regions  are  the  region 
kernel a.  The  shaded  area  shows  region  Rj.  (b)  Region  graph  corresponding  to  the  regions 
in  part  A  The  link  labels  indicate  the  existence  of  a  common  boundary  in  the  91  and/or  92 
directions 

Free  space  regions  are  non-convex  and  so  points  within  the  region  may  not  always  be 
connectable  by  a  straight  line  There  is,  however,  a  simple  method  for  moving  between 
points  within  the  region:  move  from  each  point  along  its  slice  to  the  edge  of  the  kernel 
and  connect  these  kernel  points  with  a  straight  line. 

To  search  for  a  path  between  points  in  different  regions  requires  representing  the 
connectivity  of  the  regions.  We  build  a  region  graph  where  the  nodes  are  regions  and 
the  links  indicate  regions  with  common  boundary.  Associated  with  each  region  are  a 
set  of  links  to  adjacent  regions,  each  link  records  the  area  of  overlap.  Regions  have 
neighbors  primarily  in  the  qi  direction;  for  these  neighbors,  the  range  of  92  values  at  the 
common  region  boundary  is  stored  with  the  link.  By  construction,  regions  only  have  q 2 
neighbors  at  the  0  =  2x  boundary,  anywhere  else  the  region  is  bounded  above  and  below 
by  obstacles 

In  general,  each  n  dimensional  slice  is  represented  as  a  list  of  n  -  1  dimensional 
slices  and  one  dimensional  slices  are  a  list  of  ranges  of  joint  values.  We  have  seen  that 
two  dimens  onal  regions  are  constructed  by  joining  neighboring  01  e  dimensional  1 ! ice- 
projections.  In  principle,  we  could  construct  three  dimension;  1  regions  by  joining  n  igh- 
fcoring  two  limensionai  regions,  and  so  on.  Instead,  for  thre-  dimensional  (  -spac  s  we 
simply  build  two  dimensional  regions  for  each  range  of  values  >f  the  first  join i  parai  .eter 
and  represent  the  connectivity  among  these  regions  in  the  reg  on  gr  ph  (Figu  e  14)  The 
connectivity  is  determined  by  detecting  overlap  beiween  reg  on  kernels  in  1  eighb  ring 
two  dimensional  slices,  that  is.  slices  obtained  by  incrementing  or  decrementing  the  first 
joint  parameter.  When  overlap  exists,  the  area  of  overlap  is  associated  with  the  corre- 


sponding  link  in  the  region  graph.  This  method  is  readily  extended  to  n  dimensional 
slices  by  considering  as  neighbors  slices  obtained  by  incrementing  or  decrementing  one 
of  the  first  n  -  2  joint  parameters  used  to  define  the  two  dimensional  slice. 

The  main  feature  of  this  region  representation  is  that  it  exploits  the  coherence  of  the 
free  space  thus,  for  example,  it  does  not  introduce  many  arbitrary  divisions  in  the  free- 
space  such  as  are  introduced  by  octree-type  representations  [7].  Exploiting  the  natural 
coherence  has  a  number  of  practical  advantages.  The  main  result  is  the  compactness  of 
the  representation:  very  few  regions  are  required  to  represent  rather  complex  free  spaces. 
Another  important  result  is  low  branching  in  the  region  graph:  each  region  has  relatively 
few  neighbors.  These  characteristics  of  the  representation  also  make  possible  some  of  the 
heuristic  search  technique  described  in  section  7. 

6.  Searching  for  a  Path  in  the  Region  Graph 

In  this  section,  I  describe  a  technique  for  searching  a  regio-  graph,  This  technique  applies 
to  searching  any  subset  of  the  C-space;  it  is  not  necessary  that  the  complete  C-space  be 
examined  before  any  searching  is  done.  Section  7  describes  some  heuristic  strategies  for 
limiting  what  parts  of  the  C-space  are  actually  explored. 

Path  searching  is  done  by  an  A'  search  in  the  region  graph  from  the  region  containing 
the  start  point  to  the  region  containing  the  goal  point.  During  the  search,  a  list  of  search 
nodes  is  kept.  Each  search  node  is  associated  with  some  intermediate  region  in  the  region 
graph  and  represents  a  set  of  regions  connecting  the  start  region  to  that  intermediate 
region.  For  each  node,  we  also  keep  track  of  an  entry  point  on  the  region  boundary  that 
represents  the  location  where  the  robot  path  would  enter  tue  region.  When  a  search 
node  is  expanded  by  extending  the  region  path  to  an  adjacent  region,  the  entry  point 
is  moved  to  the  closest  point  on  the  common  boundary  between  the  two  regions.  The 
entry  point  to  the  next  region  becomes  the  exit  point  for  the  current  region. 

To  carry  out  the  search  we  must  associate  with  each  search  node  an  actual  distance 
covered  and  an  underestimate  of  the  remaining  distance  to  the  goal.  We  use  the  distance 


between  entry  points  to  define  the  distance  between  two  regions  and  the  underestimate 
is  the  distance  between  the  entry  point  and  the  goal.  Of  course,  these  distances  are 
based  on  differences  between  the  joint  parameters  modulo  2 n.  Once  having  found  a  list 
of  regions  connecting  the  start  to  the  goal,  the  actual  path  is  obtained  by  connecting  the 
entry  points  and  exit  points  of  the  regions.  The  entry  point  of  the  start  region  is  the 
start  point  and  the  exit  point  of  the  goal  region  is  the  goal  point. 

A  typical  path  found  by  the  algorithm  using  the  simple  strategy  described  above  is 
shown  in  Figure  15.  The  paths  tend  to  be  jagged:  some  simple  postprocessing  to  smooth 
the  path  would  be  desirable  and  is  currently  under  investigation.  On  the  other  hand, 
because  of  the  compactness  and  low  branching  of  the  region  representation,  searching  for 
a  path  tends  to  be  very  fast  (less  than  half  a  second  for  two  dimensional  C-spaces) 

7.  Heuristic  Subsets  of  the  C-space 

Having  built  a  C-space.  it  may  be  searched  repeatedly  for  different  paths  Changes  to 
the  environment,  however,  will  cause  parts  of  the  C-space  to  be  recomputed  In  rapidlv 
changing  environments,  it  may  not  be  appropriate  to  compute  the  complete  (  space 
since  only  small  sections  of  the  C-space  will  ever  be  traversed.  This  section  describes 


Figure  16.  The  first  three  links  of  the  manipulator  of  Figure  1  The  last  three  links  and  the  end 
effector  have  been  replaced  by  a  simple  bounding  box. 


experience  with  a  number  of  simple  heuristic  strategies  that  help  select  the  subset  of  the 
C-space  relevant  to  a  particular  path. 

7.1  Decoupling  the  degrees  of  freedom 

The  path  shown  in  Figure  1  was  computed  using  two  simple  heuristics  to  subset  the 
C-space:  First  plan  a  path  for  the  first  3  links  and  a  simple  conservative  approximation 
of  the  rest  of  the  manipulator  (the  last  three  links,  the  end-effec;or  and  the  load),  see 
Figure  16.  The  origin  and  goal  for  this  path  are  chosen  to  be  the  points  in  free  space 
closest  to  the  (projection  of  the)  actual  origin  and  g  >al  Note  that  these  points  may 
differ  from  the  actual  origin  and  goal  in  all  of  the  joints  Having  found  such  a  path,  there 
remains  finding  paths  in  the  six-dimensional  C-space  between  the  actual  origin  (resp 
goal)  and  the  origin  (resp.  goal)  of  the  path  For  all  these  paths,  we  compute  only  the 
portion  of  the  C-space  bounded  by  the  joint  values  of  the  origin  and  goal  configurations 
This  strategy  has  the  effect  of  nearly  decoupling  the  degrees  of  freedom  The  six- 
dtmensional  planning  is  confined  to  the  areas  near  the  origin  and  goal.  Of  course,  this 
strategy  will  fail  to  find  a  path  in  the  worst  case,  but  this  strategy  has  proven  to  be 
reliable  and  efficient  in  most  practical  situations. 

7.2  Modifying  low-dimensional  paths 

One  alternative  approach  for  searching  for  a  path  from  configuration  {q.  ]  ?.„) 

configuration  (q}  i . gyri)  is  the  following 
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1.  Ignore  all  but  the  first  two  joints.  Build  the  free  space  for  this  reduced  manipula¬ 
tor.  Find  a  sequence  of  free  space  regions  that  contain  a  path  from  (qf  \,qf  2)  to 
(9*,i »«*.*)•  Ut  *  ~  3 

2.  Expand  the  portion  of  the  free  space  included  in  the  regions  found  60  far  to  incor¬ 
porate  the  link  «.  That  is,  quantize  the  range  of  values  in  the  region  and  use  them 
to  compute  one  dimensional  slice  projections  of  the  C-space  for  the  first  1  links. 

3.  Search  for  a  sequence  of  free  space  regions  that  contain  a  path  from  (q,j, .  .  ,q, ,) 
to  [qg  i,.  ..,qg  t).  If  i  —  m  then  stop;  else  increment  t  and  go  to  step  2 


This  strategy  is  illustrated  graphically  in  Figure  17  The  idea  is  to  focus  on  rele¬ 
vant  sections  of  the  configuration  space  by  first  finding  paths  for  successively  “longer’’ 
manipulators,  starting  with  a  two-link  manipulator  and  going  all  the  way  through  to  a 
manipulator  with  n  links. 

It  is  important  that  at  each  stage  we  consider  not  just  a  single  path  of  the  “shorter” 
manipulator,  but  a  sequence  of  regions  that  span  all  the  free-spare  between  a  set  of 
obstacles.  The  addition  of  link  n  will  typically  change  the  equir "d  path  for  the  first 
n  -  1  links  Therefore  the  search  S}  ace  should  include  more  han  ;  single  p  <th  so  s  to 
avoid  the  need  to  perform  a  backtracking  sear  h. 

This  method  has  1  een  implemented.  The  path  for  the  fon  degr  e-of-free<  om  m  nip- 
ulator  show  n  in  Figure  18  was  found  tn  this  te. ‘iniqui  The  t>  hniq  e  leads  t.  sigrii'  ant 
time  savings  on  problems  involving  more  than  two  degrees  01  freedom  Of  course,  -ince 
the  complete  configuration  space  is  not  computed,  the  time  to  plan  a  subsequent  motion 
in  the  same  workspace  will  be  as  long  as  that  for  the  initial  motion 


8.  D  iscussion 

The  main  advantages  of  the  algorithm  described  here  are  it  is  simple  to  implement, 
it  is  fast  for  manipulators  with  few  degrees  of  freedom,  it  can  deal  with  manipulators 
having  many  degrees  of  freedom  including  redundant  manipulators,  and  it  can  deal  with 
cluttered  environments  and  non-convex  polyhedral  obstacles  The  total  wall-clock  time 
to  compute  the  C-space  obstacles  and  then  plan  a  path  for  the  two-link  example  shown 
in  Figure  4  and  15  is  six  seconds  on  a  Symbolics  3600  Lisp  Machine  with  floating-point 
operations  performed  in  software  These  times  could  be  improved  by  carefully  re-coding 
the  algorithm,  but  they  are  already  quite  a  bit  faster  than  a  human  using  an  interactive 
programming  system  (on-line  or  off-line) 

The  main  disadvantages  of  the  algorithm  are  the  approximations  introduced  by  the 
quantization  may  cause  the  algorithm  to  miss  legal  paths  in  very  tight  environments,  and 
the  rapid  growth  in  execution  time  with  the  number  of  robot  joints  This  last  drawback 
is  probably  inherent  in  any  general  motion  planner,  the  worst -case  time  bound  will  be 
exponential  in  the  number  of  degrees  of  freedom  19 

The  performance  of  this  algorithm  shows  that  motion  planning  algorit  hms  can  be  fast 
enough  and  simple  enough  for  practical  use  I  believe  t  hat  in  many  applications  automat ic 


Figure  17.  It  is  possible  to  alternate  between  searching  for  a  path  and  building  the  C-space.  (a) 
A  three  degree  of  freedom  problem.  Paths  for  reduced  manipulators  (b)  limit  which  part  of  the 
full-dimensional  C-space  (c)  needs  to  be  searched. 

motion  planning  will  be  more  time  effective  than  interactive  off-line  programming  of 
robots  In  fact,  the  planning  times  will  probably  be  on  the  order  of  the  times  required 
to  perform  hidden  surface  elimination  in  graphics  systems 
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Appendix:  Computing  contact  angles  for  polyhedra 


In  what  follows  we  assume  that  we  are  dealing  with  a  convex  polyhedron  describing  link 
k  and  an  obstacle  polyhedron  (not  necessarily  convex).  The  coordinate  system  is  chosen 
so  that  the  origin  corresponds  to  the  position  of  revolute  joint  k  and  the  z  axis  is  aligned 
with  the  joint  axis  (Figure  19).  The  coordinate  representation  of  all  vectors  is  relative 
to  this  coordinate  system.  We  assume  that  the  initial  position  of  the  link  polyhedron 
corresponds  to  qk  =  0.  We  are  interested  in  computing  values  of  qk  for  which  the  link  is 
in  contact  with  the  obstacle  polyhedron. 

Type  B  contact:  vertex  of  link  and  face  of  obstacle 

We  are  given  a  vertex  of  the  link  whose  position  vector  is  v  and  an  obstacle  face  whose 
plane  equation  is  n  ■  x  -  d  —  0  (n  is  the  plane’s  outward-facing  unit  normal).  We  solve 
for  the  angle  qk  that  rotates  the  vector  onto  the  plane.  We  obtain  the  equation  for  qk  by 
sub-titutin*  the  vertex's  [  isition,  rotated  by  qk,  into  the  plane  equation  and  solving  for 
9  k 

The  coordinates  of  the  position  vector  for  the  rotated  vertex  are: 

v'  =  {vxcosqk  -  vys'\nqk,vxsinqk  +  ty  cos qk,vx) 

Substituting  into  the  plane  equation  yields  n-v'-d  =  0,  this  yields  a  simple  trigonometric 
equation 


(nxvz  -  nvfy)  cos qk  (nvvx  -  nxvy)  sin  qk~  -d  -  nxvx 
whose  solution  is  Eq  (6). 


(5) 


/ 


qk  ~  cos 


-nzvx  -  d 


\  \  (l'l  +  tyM1  -  *1) 


atan(nvvI:  -  nxvv,nxvx  +  nvvy)  (6) 


Eq  (5)  is  the  equation  for  a  type  B  C-surface  14  .  Note  that  if  we  let  n,  =  0,  then 
Eq  (5)  and  Eq  (1)  are  essentially  identical.  The  arctangent  simply  computes  the  angle 
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between  the  plane  normal  and  the  projection  of  v  on  the  xy  plane;  this  magnitude  is 
analogous  to  4>  in  the  planar  case.  Eq.  (6)  and  Eq.  (2)  are  also  related  in  the  same  way. 

The  left  hand  side  of  the  Eq.  (5)  represents  the  perpendicular  distance  of  the  rotated 
vertex  from  the  obstacle  plane.  The  sign  of  the  derivative  of  this  quantity  with  respect 
to  qk  can  be  used  to  determine  whether  increasing  or  decreasing  qk  causes  a  collision. 

The  orientation  constraint  simply  requires  testing  whether  the  other  endpoint  of  all 
the  edges  meeting  at  the  contact  vertex  are  on  the  outside  of  the  plane.  This  is  done  by 
substituting  the  position  vector  of  these  endpoints  into  the  left  hand  side  of  the  plane 
equation  and  testing  that  the  value  is  positive. 

Type  A  contact:  vertex  of  obstacle  and  face  of  link 

We  are  given  an  obstacle  vertex  whose  position  vector  is  v  and  a  link  face  whose  plane 
equation  is  n  •  x  +  d  =  0  (n  is  the  plane’s  outward -facing  normal).  The  solution  for  qk  is 
almost  identical  to  the  type  A  case,  the  only  difference  is  the  sign  of  the  first  argument 
to  the  arctangent.  This  reflects  the  fact  that  in  type  A  contact  we  are  treating  the  link 
as  stationary  and  assuming  the  object  is  rotating  in  the  opposite  direction.  This  changes 
the  sign  of  the  sine  of  the  angle. 


Type  C  contact:  edge  of  obstacle  and  edge  of  link 

This  case  is  substantially  more  difficult;  we  follow  the  derivation  in  ,lj.  We  represent 
points  on  the  edges  parametrically  in  t.  Therefore,  points  on  the  link’s  edge  are  repre¬ 
sented  by  t; m  +  v  where  v  is  the  position  vector  of  one  of  the  endpoints  of  the  edge  and 
mis  a  vector  along  the  edge  (actually  the  difference  vector  between  the  endpoints).  The 
parameter  t;  6  [0, 1’  parameterizes  position  along  the  edge.  We  can  represent  the  vector 
along  the  obstacle  edge  similarly  as  t„u  w  for  ta  €  [0,  I  j. 

As  the  edge  rotates  around  the  z  axis,  points  on  the  edge  trace  out  circles.  The 
equation  for  points  on  those  circles  are: 

a-2  v2  -  [mxt I  -j-  o*)2  +  [rriyti  -f  v„)2 

2  =  mzti  -  t  j. 

These  can  be  combined  by  solving  the  second  equation  for  tj  =  and  substituting 

into  the  first  to  obtain: 


-(=*(- 

\  mz 


.) 


m. 


(r  -  r,)  -  t>. 


(") 


This  is  an  implicit  equation  for  poin  s  on  the  r  >tation  surface 

The  parametric  form  of  tne  obstacle  edge  can  be  used  t<  solv  for  the  intersc  ion 
of  the  edge  with  the  rotation  eurfaci 


x  =  nxtc  -  v>T,  y  -  nyt„  +  uv,  2  ---  nztv  -  u>t 
Substituting  into  Eq.  (7)  gives  a  quadratic  equation  in 


Define  the  following  terms: 

P  =  (n\  -  »l)m\  -  [ml  ~  »$)nl 

q  =  2  (nxu>x  -  nyvjv)m\  -  (m2  -r  m2)(u*  -  v*)n,  -  (mxvx  +  mvrv)mIn,| 
r  =  (w\  -  tn2)m2  -  j(m3(ti;z  -  v,)  -  t^m,)2  4-  (my(u>,  -  v*)  v,tn,)!' 

The  quadratic  equation  that  must  be  solved  for  t0  is 

Ptl  4  qta  ~  r  =  0 

Having  t0  we  can  solve  for  f|  since  we  know  that  the  z  values  at  contact  must  be  equal. 
Therefore. 

nzta  -  « >z-  vt 

tt  — - 

mx 

Given  values  for  ti  and  ta,  we  must  first  check  that  they  are  in  the  range  [0, 1]  (the  in-edge 
constraint),  then  we  can  compute  points  of  intersection  on  each  of  the  edges.  Let  1  be 
the  position  vector  of  the  intersection  point  on  the  link  edge  and  o  the  position  of  the 
intersection  point  on  the  object  edge.  Then, 

9fc  =  atan(/Ioy  -  lyox,lxox  4/yoy) 

Note,  however,  that  we  have  assumed,  when  deriving  Eq.  (7)  that  mz  /  0.  In  the 
not  uncommon  event  that  it  is,  then  all  the  points  on  the  rotation  surface  have  z  —  vz 
and  so  will  the  intersection  point  with  the  obstacle  edge  We  can  use  this  to  obtain 
t We  can  heri  !ve  for  by  i>ing  the  f  ct  that  the  contact  point  on  the  link 
edg<  vill  b  on  the  me  i  rcle  as  t  he  contact  point  on  the  obsta.  le  edge: 

(rnxt,  t* j )  -  (mytt  -  rvV  -  (nxtc  wT)2  -  (nyt  -  u-„)2 

But  we  know  the  \  due  of  t,,  so  this  is  a  quadratic  equation  for  / /.  Given  the  values  of 
and  t,  we  can  solve  for  q k  as  above 

In  addition  to  solving  for  the  value  of  the  contact  angle  we  must  compute  the  deriva¬ 
tive  of  the  distance  to  the  contact  as  a  function  of  qk.  As  before,  this  will  determine 
whether  the  contact  angle  is  a  potential  upper  or  lower  bound  for  a  contact  range.  Un¬ 
fortunately.  this  is  not  as  simple  as  it  is  for  type  A  and  B  contacts. 

When  the  two  edges  are  in  contact,  any  motion  component  perpendicular  to  both  of 
them  will  cause  a  collision  while  a  component  of  motion  along  either  edge  will  not  cause 
a  collision.  The  direcion  perpendicular  to  both  edges  is  simply  the  cross  product  of  the 
two  edge  vectors  (given  the  link  edge  rotated  to  the  contact  angle): 

c(qk)  --  m(qk)  x  n 

The  qk  dependence  has  been  indicated  explicitly. 

One  problem  here  is  that  we  do  not  know  whether  c  is  outward  pointing  or  not.  We 
can  decide  that  by  dotting  c  with  a  direction  known  to  point  into  the  link.  If  ej  and 
are  the  direction  vectors  of  edges  meeting  the  link  edge  at  one  of  its  vertices  (see  Figure 


Figure  20.  The  definition  of  the  a,  for  t  =  1,2,3, 4 


20),  then  +  «j  is  a  direction  pointing  into  the  link  volume.  Let  k  =  ngn(c  •  («i  +  *j)), 
then  kc  is  the  outward-pointing  normal  we  require  (see  [5]  for  a  careful  derivation). 
Given  the  value  of  k,  the  type  C  C-surface  equation  can  be  written  as  [14]: 

*c(ffc)-(w-v(9t))  =  0  (8) 

Differentiating  the  left  hand  side  with  respect  to  qk  yields 

((iu(m,  -  uzmy)nz  +  (dzmz  -  t>zmz)ny  -  ( damy  -  vym,)nI)lrsin qk+ 

((u.ymy  -i-  wxmx)nx  -r  (d,my  -  vym„)ny  -  {dMms  -  vxmM)nz)k  cos  qk 

where  dM  =  v,  ~  wM.  If  this  derivative  is  negative  then  increasing  qk  will  cause  a  collision. 

We  are  not  done  yet.  We  must  guarantee  that  the  contact  satisfies  the  orientation 
constraint.  The  following  are  necessary  and  sufficient  conditions  for  this  [5]: 

s  =  sign(c  •  «i)  =  sign(c  •  e,) 
s'  =  sign(c  •  es)  =  sign(c  •  e«) 

These  conditions  are  analogous  to  the  type  A  and  B  cases. 

Although  the  derivation  of  the  type  C  case  is  a  bit  involved,  the  actual  amount  of 
computation  involved  is  not  large. 


